﻿#include <occmesh/api/param.h>
#include <occmesh/api/utils.h>

std::vector<std::vector<double>> OCCUtils::gaussNodes{
    {0.5773502691896257, -0.5773502691896257},                                               // 2 个节点
    {0.7745966692414834, 0.0, -0.7745966692414834},                                          // 3 个节点
    {0.8611363115940526, 0.3399810435848563, -0.3399810435848563, -0.8611363115940526},      // 4 个节点
    {0.9061798459386640, 0.5384693101056831, 0.0, -0.5384693101056831, -0.9061798459386640}, // 5 个节点
    {0.9324695142031521, 0.6612093864662645, 0.2386191860831969, -0.2386191860831969, -0.6612093864662645,
     -0.9324695142031521}, // 6 个节点
    {0.9491079123427585, 0.7415311855993945, 0.4058451513773972, 0.0, -0.4058451513773972, -0.7415311855993945,
     -0.9491079123427585}, // 7 个节点
    {0.9602898564975363, 0.7966664774136267, 0.5255324099163290, 0.1834346424956498, -0.1834346424956498,
     -0.5255324099163290, -0.7966664774136267, -0.9602898564975363}, // 8 个节点
    {0.9681602395076261, 0.8360311073266358, 0.6133714327005904, 0.3242534234038089, 0.0, -0.3242534234038089,
     -0.6133714327005904, -0.8360311073266358, -0.9681602395076261}, // 9 个节点
    {0.9739065285171717, 0.8650633666889845, 0.6794095682990244, 0.4333953941292472, 0.1488743389816312,
     -0.1488743389816312, -0.4333953941292472, -0.6794095682990244, -0.8650633666889845,
     -0.9739065285171717} // 10 个节点
};

std::vector<std::vector<double>> OCCUtils::gaussWeights{
    {1.0, 1.0},                                                                                           // 2 个节点
    {0.5555555555555556, 0.8888888888888888, 0.5555555555555556},                                         // 3 个节点
    {0.3478548451374538, 0.6521451548625461, 0.6521451548625461, 0.3478548451374538},                     // 4 个节点
    {0.2369268850561891, 0.4786286704993665, 0.5688888888888889, 0.4786286704993665, 0.2369268850561891}, // 5 个节点
    {0.1713244923791704, 0.3607615730481386, 0.4679139345726910, 0.4679139345726910, 0.3607615730481386,
     0.1713244923791704}, // 6 个节点
    {0.1294849661688697, 0.2797053914892766, 0.3818300505051189, 0.4179591836734694, 0.3818300505051189,
     0.2797053914892766, 0.1294849661688697}, // 7 个节点
    {0.1012285362903763, 0.2223810344533745, 0.3137066458778873, 0.3626837833783620, 0.3626837833783620,
     0.3137066458778873, 0.2223810344533745, 0.1012285362903763}, // 8 个节点
    {0.0812743883615744, 0.1806481606948574, 0.2606106964029354, 0.3123470770400029, 0.3302393550012598,
     0.3123470770400029, 0.2606106964029354, 0.1806481606948574, 0.0812743883615744}, // 9 个节点
    {0.0666713443086881, 0.1494513491505806, 0.2190863625159820, 0.2692667193099963, 0.2955242247147529,
     0.2955242247147529, 0.2692667193099963, 0.2190863625159820, 0.1494513491505806, 0.0666713443086881} // 10 个节点
};

std::vector<double> OCCUtils::gaussNodes64{
    -0.024350293, 0.024350293, -0.072993122, 0.072993122, -0.121462819, 0.121462819, -0.16964442,  0.16964442,
    -0.217423644, 0.217423644, -0.264687162, 0.264687162, -0.311322872, 0.311322872, -0.357220158, 0.357220158,
    -0.402270158, 0.402270158, -0.446366017, 0.446366017, -0.489403146, 0.489403146, -0.531279464, 0.531279464,
    -0.571895646, 0.571895646, -0.611155355, 0.611155355, -0.648965471, 0.648965471, -0.685236313, 0.685236313,
    -0.71988185,  0.71988185,  -0.752819907, 0.752819907, -0.783972359, 0.783972359, -0.813265315, 0.813265315,
    -0.840629296, 0.840629296, -0.865999398, 0.865999398, -0.889315446, 0.889315446, -0.910522137, 0.910522137,
    -0.929569172, 0.929569172, -0.946411375, 0.946411375, -0.9610088,   0.9610088,   -0.973326828, 0.973326828,
    -0.983336254, 0.983336254, -0.991013371, 0.991013371, -0.996340117, 0.996340117, -0.999305042, 0.999305042};

std::vector<double> OCCUtils::gaussWeights64{
    0.048690957, 0.048690957, 0.048575467, 0.048575467, 0.048344762, 0.048344762, 0.047999389, 0.047999389,
    0.047540166, 0.047540166, 0.046968183, 0.046968183, 0.046284797, 0.046284797, 0.045491628, 0.045491628,
    0.044590558, 0.044590558, 0.043583725, 0.043583725, 0.042473515, 0.042473515, 0.041262563, 0.041262563,
    0.039953741, 0.039953741, 0.038550153, 0.038550153, 0.037055129, 0.037055129, 0.035472213, 0.035472213,
    0.033805162, 0.033805162, 0.032057928, 0.032057928, 0.030234657, 0.030234657, 0.028339673, 0.028339673,
    0.02637747,  0.02637747,  0.024352703, 0.024352703, 0.022270174, 0.022270174, 0.020134823, 0.020134823,
    0.017951716, 0.017951716, 0.01572603,  0.01572603,  0.013463048, 0.013463048, 0.011168139, 0.011168139,
    0.00884676,  0.00884676,  0.006504458, 0.006504458, 0.004147033, 0.004147033, 0.001783281, 0.001783281};

std::vector<gp_Vec> OCCUtils::OBB(const std::vector<gp_Pnt> &points)
{
    int n = points.size();

    // 坐标分离为 3 个随机变量
    Eigen::VectorXd X(n), Y(n), Z(n);
    for (int i = 0; i < n; i++)
    {
        X[i] = points[i].X();
        Y[i] = points[i].Y();
        Z[i] = points[i].Z();
    }

    // 计算期望点
    double E[] = {0, 0, 0};
    E[0] = X.sum() / n;
    E[1] = Y.sum() / n;
    E[2] = Z.sum() / n;

    // 计算协方差矩阵
    Eigen::MatrixXd B(3, 3);
    B.setZero();
    for (int i = 0; i < n; i++)
    {
        B(0, 0) += X[i] * X[i];
        B(0, 1) += X[i] * Y[i];
        B(0, 2) += X[i] * Z[i];

        B(1, 0) += Y[i] * X[i];
        B(1, 1) += Y[i] * Y[i];
        B(1, 2) += Y[i] * Z[i];

        B(2, 0) += Z[i] * X[i];
        B(2, 1) += Z[i] * Y[i];
        B(2, 2) += Z[i] * Z[i];
    }
    B = B / n;

    // 计算 Xi_i 和 Xi_j 的协方差
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            B(i, j) -= E[i] * E[j];

    // 特征分解
    Eigen::EigenSolver<Eigen::MatrixXd> es(B);

    // 获得特征值，取实部
    Eigen::VectorXcd cval = es.eigenvalues();
    Eigen::VectorXd val(cval.size());
    for (int i = 0; i < val.size(); i++)
        val[i] = cval[i].real();

    // 获得特征向量的矩阵，取实部
    Eigen::MatrixXcd cV = es.eigenvectors();
    Eigen::MatrixXd V(cV.rows(), cV.cols());
    for (int i = 0; i < V.rows(); i++)
        for (int j = 0; j < V.cols(); j++)
            V(i, j) = cV(i, j).real();

    // 比较对角元，进行排序
    int order[] = {0, 1, 2};
    for (int i = 0; i < 3; i++)
    {
        for (int j = 1; j < 3; j++)
        {
            // 比较相邻的元素，如果后者大于前者就交换
            if (val[j] > val[j - 1])
            {
                std::swap(order[j - 1], order[j]);
                std::swap(val[j - 1], val[j]);
            }
        }
    }

    // 三个主方向，按照从大到小的顺序
    gp_Vec x(V(0, order[0]), V(1, order[0]), V(2, order[0]));
    gp_Vec y(V(0, order[1]), V(1, order[1]), V(2, order[1]));
    gp_Vec z(V(0, order[2]), V(1, order[2]), V(2, order[2]));
    x.Normalize();
    y.Normalize();
    z.Normalize();

    // 计算在主方向上的投影长度，需要将期望平移到原点
    gp_Vec Ev(E[0], E[1], E[2]);
    Eigen::MatrixXd M(3, 2);
    M.setZero();
    for (int i = 0; i < n; i++)
    {
        gp_Vec v = gp_Vec(X[i], Y[i], Z[i]) - Ev;

        double p[] = {v.Dot(x), v.Dot(y), v.Dot(z)};
        for (int j = 0; j < 3; j++)
        {
            if (M(j, 0) > p[j])
                M(j, 0) = p[j];
            if (M(j, 1) < p[j])
                M(j, 1) = p[j];
        }
    }

    // 记录在 x,y,z 正反方向的 6 个向量，以及中心向量 Ev
    std::vector<gp_Vec> box;
    for (int j = 0; j < 2; j++)
        box.push_back(x * M(0, j));
    for (int j = 0; j < 2; j++)
        box.push_back(y * M(1, j));
    for (int j = 0; j < 2; j++)
        box.push_back(z * M(2, j));
    box.push_back(Ev);

    // 调整法向方向
    if (box[4].Dot({0, 1, 0}) < 0)
        for (int i = 0; i < 5; i++)
            box[i] = box[i] * -1;

    // 调整为右手坐标系，两个方向的先后顺序会决定光照时的法向
    if (box[4].Dot(box[0].Crossed(box[2])) < 0)
    {
        gp_Vec tx = box[0];
        box[0] = box[2];
        box[2] = tx;

        // 注意正反方向的都要交换
        tx = box[1];
        box[1] = box[3];
        box[3] = tx;
    }

    return box;
}

int OCCUtils::FindSpan(double u, int p, const std::vector<double> &knots)
{
    // 防止越界
    assert(u >= knots[p]);
    assert(u <= knots[knots.size() - p - 1]);

    // 返回左闭右开区间 u \in [u_i, u_{i+1})
    int m = knots.size() - 1;
    int n = m - p - 1;

    // 边界情况：如果 u 在区间最大值的边界上，就返回左边的区间
    // 注意这里边界是最大的值，而不是节点最大值
    if (u == knots[knots.size() - p - 1])
        return n;

    // 二分法寻找区间
    int low = p;
    int high = n + 1;
    int mid = (low + high) / 2;
    while (u < knots[mid] || u >= knots[mid + 1])
    {
        if (u < knots[mid])
            high = mid;
        else
            low = mid;
        mid = (low + high) / 2;
    }
    return mid;
}

std::vector<double> OCCUtils::BasisFuncs(int i, double u, int p, const std::vector<double> &knots)
{
    // 防止越界，确保索引在有效区间内
    assert(i >= p);
    assert(i < int(knots.size() - p - 1));
    assert(u >= knots[p]);
    assert(u <= knots[knots.size() - p - 1]);

    std::vector<double> N(p + 1);     // 存放 u \in [u_i, u_{i+1}) 处不为零的基函数值
    std::vector<double> left(p + 1);  // left[j] = u - u_{i+1-j}
    std::vector<double> right(p + 1); // right[j] = u_{i+j} - u

    N[0] = 1;
    for (int j = 1; j < p + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        double saved = 0;
        for (int r = 0; r < j; r++)
        {
            double tmp = N[r] / (right[r + 1] + left[j - r]);
            N[r] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        N[j] = saved;
    }
    return N;
}

std::vector<std::vector<double>> OCCUtils::DerivBasisFuncs(int d, int i, double u, int p,
                                                           const std::vector<double> &knots)
{
    // 排除无效参数
    assert(d <= p);
    assert(i >= p);
    assert(i < int(knots.size() - p - 1));
    assert(u >= knots[p]);
    assert(u <= knots[knots.size() - p - 1]);

    // ders[k][j] 表示 N_{i-p+j}^(k)
    std::vector<std::vector<double>> ders(d + 1, std::vector<double>(p + 1));

    // 先计算出非零基函数及节点差分
    std::vector<std::vector<double>> ndu(p + 1, std::vector<double>(p + 1));
    std::vector<double> left(p + 1);  // left[j] = u - u_{i+1-j}
    std::vector<double> right(p + 1); // right[j] = u_{i+j} - u

    ndu[0][0] = 1;
    for (int j = 1; j < p + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        double saved = 0;
        for (int r = 0; r < j; r++)
        {
            // 下三角部分存放节点差分
            // ndu[j][r] = u_{i + r + 1} - u_{i + r + 1 - j}
            ndu[j][r] = right[r + 1] + left[j - r];

            // 上三角部分（包括对角元）存放所有非零不大于 p 次基函数（AllBasisFuncs）
            // 注意这里 ndu[r][j] = N_{r,j}，没有像 AllBasisFuncs 一样反过来，因为空间已经分配好
            double tmp = ndu[r][j - 1] / ndu[j][r];
            ndu[r][j] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        ndu[j][j] = saved;
    }

    // 将计算结果中的基函数部分（上三角）记录下来
    for (int j = 0; j < p + 1; j++)
        ders[0][j] = ndu[j][p];

    // N_{i,p}^(k) = p * [ N_{i,p-1}^(k-1) / (u_{i+p} - u_i) - N_{i+1,p-1}^(k-1) / (u_{i+p+1}-u_{i+1}) ]
    std::vector<std::vector<double>> a(2, std::vector<double>(p + 1));
    for (int r = 0; r < p + 1; r++)
    {
        // 计算 N_{i,r}^(k)
        double s1 = 0;
        double s2 = 1;

        // a_{0,0} = 1
        a[0][0] = 1;
        for (int k = 1; k < d + 1; k++)
        {
            double der = 0;

            // a_{k,0} = a_{k-1,0} / (u_{i+p-k+1} - u_i)
            // 从 N_{i-p+r,p}^(k) 开始计算，这里 "i" 是 i-p+r
            // 所以取 ndu[p - k + 1][r - k] = u_{i+r-k+1} - u_{i-p+r+j}
            if (r >= k)
            {
                // a[s2] 存放 a_{k, }, a[s1] 存放 a_{k-1, }
                a[s2][0] = a[s1][0] / ndu[p - k + 1][r - k];
                der = a[s2][0] * ndu[r - k][p - k];
            }

            // 正常情况下 j1 = 1, j2 = k - 1，就是完整的计算求和
            // 特殊情况下要排除掉系数 a 为零和基函数 N 为零的情况
            int j1, j2;
            if (r - k >= -1)
                j1 = 1;
            else
                j1 = -r + k;

            if (r - 1 <= p - k)
                j2 = k - 1;
            else
                j2 = p - r;

            // a_{k,j} = (a_{k-1,j} - a_{k-1,j-1}) / (u_{i+p+j-k+1} - u_{i+j})
            // 注意这里 "i" 是 i-p+r
            // j = 1, ..., k-1
            for (int j = j1; j < j2 + 1; j++)
            {
                a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[p - k + 1][r - k + j];
                der += a[s2][j] * ndu[r - k + j][p - k];
            }

            // 计算对角 a_{k,k} = -a_{k-1,k-1} / (u_{i+p+1} - u_{i+k})
            // 注意这里 "i" 是 i-p+r
            if (r <= p - k)
            {
                a[s2][k] = -a[s1][k - 1] / ndu[p - k + 1][r];
                der += a[s2][k] * ndu[r][p - k];
            }
            ders[k][r] = der;

            // 交换行，将之前计算的 a_{k, } 作为下一个循环的 a_{k-1, }
            int s = s1;
            s1 = s2;
            s2 = s;
        }
    }

    // 由于重复求导，乘上因子 p * (p - 1) * ... * (p - k + 1)
    int r = p;
    for (int k = 1; k < d + 1; k++)
    {
        for (int j = 0; j < p + 1; j++)
            ders[k][j] *= r;
        r *= p - k;
    }
    return ders;
}

Handle(Geom2d_Curve) OCCUtils::Interpolate(const std::vector<gp_Pnt2d> &points)
{
    auto param = ChordParameterize(points);
    Handle(TColStd_HArray1OfReal) Params = new TColStd_HArray1OfReal(1, points.size());
    Handle(TColgp_HArray1OfPnt2d) Points = new TColgp_HArray1OfPnt2d(1, points.size());
    for (std::size_t i = 0; i < points.size(); i++)
    {
        Params->SetValue(i + 1, param[i]);
        Points->SetValue(i + 1, points[i]);
    }
    auto inter = Geom2dAPI_Interpolate(Points, Params, false, 1e-16);
    inter.Perform();
    return inter.Curve();
}

void OCCUtils::CompatCurves(Handle(Geom_BSplineCurve) c1, Handle(Geom_BSplineCurve) c2)
{
    // 首先升阶
    int d = std::max(c1->Degree(), c2->Degree());

    c1->IncreaseDegree(d);
    c2->IncreaseDegree(d);

    // 统一节点
    Standard_Real first1 = c1->FirstParameter();
    Standard_Real first2 = c2->FirstParameter();
    Standard_Real last1 = c1->LastParameter();
    Standard_Real last2 = c2->LastParameter();

    TColStd_Array1OfReal knots(1, c1->NbKnots());
    Standard_Real a = (last2 - first2) / (last1 - first1);
    Standard_Real b = first2 - a * first1;
    for (int i = 1; i <= c1->NbKnots(); i++)
    {
        // 由于 SetKnot 不能设置超出节点区间的值，因此只能先保存再整体修改
        knots.SetValue(i, c1->Knot(i) * a + b);
    }
    c1->SetKnots(knots);

    int i = 1, j = 1;
    while (i <= c2->NbKnots() && j <= c1->NbKnots())
    {
        if (std::abs(c1->Knot(j) - c2->Knot(i)) < 1e-6)
        {
            int m = std::max(c2->Multiplicity(i), c1->Multiplicity(j));
            c2->InsertKnot(c2->Knot(i), m - c2->Multiplicity(i));
            c1->InsertKnot(c1->Knot(j), m - c1->Multiplicity(j));
        }
        else if (c1->Knot(j) - c2->Knot(i) < 1e-6)
            c2->InsertKnot(c1->Knot(j), c1->Multiplicity(j));
        else if (c1->Knot(j) - c2->Knot(i) > 1e-6)
            c1->InsertKnot(c2->Knot(i), c2->Multiplicity(i));
        i++, j++;
    }
}

std::vector<double> OCCUtils::ChordParameterize(const std::vector<gp_Pnt2d> &points)
{
    std::size_t n = points.size();
    std::vector<double> para(n);
    para[0] = 0;

    double total = 0;
    for (std::size_t i = 1; i < n; i++)
        total += points[i].Distance(points[i - 1]);

    for (std::size_t i = 1; i < n - 1; i++)
    {
        double di = points[i].Distance(points[i - 1]);
        para[i] = para[i - 1] + di / total;
    }
    para[n - 1] = 1.0;
    return para;
}

std::vector<gp_Pnt2d> OCCUtils::ProjectParameterize(const std::vector<gp_Pnt> &points)
{
    // 计算有向包围盒
    std::vector<gp_Vec> box = OCCUtils::OBB(points);

    // 最长的两个方向，构造初始投影平面
    std::vector<gp_Vec> oxy(3);
    oxy[0] = box.back() + box[1] + box[3];
    oxy[1] = box[0].Normalized();
    oxy[2] = box[2].Normalized();

    // 计算初始投影参数
    int N = points.size();
    std::vector<gp_Pnt2d> params(N);
    for (int i = 0; i < N; i++)
    {
        gp_Vec a(points[i].XYZ());
        params[i] = gp_Pnt2d(oxy[1].Dot(a - oxy[0]), oxy[2].Dot(a - oxy[0]));
    }
    return params;
}

TopoDS_Wire OCCUtils::TrimmSurface(Handle(Geom_Surface) surface, const std::vector<Handle(Geom2d_Curve)> &curves)
{
    std::vector<TopoDS_Edge> anEdges;
    for (int i = 0; i < curves.size(); i++)
    {
        auto tmin = curves[i]->FirstParameter();
        auto tmax = curves[i]->LastParameter();
        anEdges.push_back(BRepBuilderAPI_MakeEdge(curves[i], surface, tmin, tmax));
    }

    TopTools_ListOfShape aOrderedEdges;
    for (int e = 0; e < anEdges.size(); ++e)
    {
        const TopoDS_Edge &anEdge = anEdges[e];
        aOrderedEdges.Append(anEdge);
    }

    BRepBuilderAPI_MakeWire bdWire;
    bdWire.Add(aOrderedEdges);

    if (bdWire.IsDone())
        return bdWire.Wire();
    else
        return TopoDS_Wire{};
}

double OCCUtils::ProjectWithOCC(Handle(Geom_Surface) surf, gp_Pnt P, double &u, double &v)
{
    GeomAPI_ProjectPointOnSurf Proj(P, surf);
    Proj.LowerDistanceParameters(u, v);
    return Proj.LowerDistance();
}

double OCCUtils::ProjectWithNearest(Handle(Geom_Surface) surface, gp_Pnt P, double &u, double &v, double tol)
{
    // 应用牛顿迭代法
    int times = 0;
    double U1, U2, V1, V2;
    surface->Bounds(U1, U2, V1, V2);

    int maxIt = 10;
    double dp = tol * 10;
    gp_Pnt Q = surface->Value(u, v);
    double near = P.Distance(Q);
    while (near > tol && dp > tol && times++ < maxIt)
    {
        gp_Vec DU, DV, DUU, DUV, DVV;
        surface->D2(u, v, Q, DU, DV, DUU, DVV, DUV);

        auto dS = Q.XYZ() - P.XYZ();
        double lSu = DU.Magnitude();
        double lSv = DV.Magnitude();

        Eigen::MatrixXd J(2, 2);
        J(0, 0) = pow(lSu, 2) + DUU * dS;
        J(0, 1) = J(1, 0) = DU * DV + DUV * dS;
        J(1, 1) = pow(lSv, 2) + DVV * dS;

        Eigen::VectorXd kappa(2);
        kappa[0] = -(DU * dS);
        kappa[1] = -(DV * dS);

        // 注意 J 不一定正定，因此使用标准解法
        Eigen::VectorXd duv = J.inverse() * kappa;
        u += duv[0];
        v += duv[1];

        // 防止越界
        if (u <= U1)
            u = U1 + tol;
        if (u >= U2)
            u = U2 - tol;
        if (v <= V1)
            v = V1 + tol;
        if (v >= V2)
            v = V2 - tol;

        near = P.Distance(surface->Value(u, v));
        dp = std::abs(lSu * duv[0] + lSv * duv[1]);
    }
    Q = surface->Value(u, v);
    return near;
}

double OCCUtils::ProjectWithOrtho(Handle(Geom_Surface) surf, gp_Pnt P, double &u, double &v, double tol)
{
    double U1, U2, V1, V2;
    surf->Bounds(U1, U2, V1, V2);

    // 曲率投影
    int times = 0;
    double angle = tol * 10;
    Eigen::VectorXd uv(2);
    uv[0] = (U1 + U2) / 2;
    uv[1] = (V1 + V2) / 2;

    int maxIt = 10;
    while (angle > tol && times < maxIt)
    {
        // 计算各阶导向量
        gp_Pnt S00;
        gp_Vec S10, S01, S20, S02, S11;
        surf->D2(uv[0], uv[1], S00, S10, S01, S20, S02, S11);

        // 投影方向
        gp_Vec dP = gp_Vec(P.XYZ()) - gp_Vec(S00.XYZ());

        // 获得 I,II 基本形式的矩阵
        gp_Vec N = S10.Crossed(S01).Normalized();

        Eigen::MatrixXd I(2, 2);
        I(0, 0) = S10.Dot(S10);
        I(0, 1) = I(1, 0) = S10.Dot(S01);
        I(1, 1) = S01.Dot(S01);

        Eigen::MatrixXd II(2, 2);
        II(0, 0) = N.Dot(S20);
        II(0, 1) = II(1, 0) = N.Dot(S11);
        II(1, 1) = N.Dot(S02);

        // 沿着 Su Sv 方向进行分解（利用线性方程组）
        Eigen::VectorXd b(2);
        b[0] = dP.Dot(S10);
        b[1] = dP.Dot(S01);

        // Cholesky 分解进行求解
        Eigen::LLT<Eigen::MatrixXd> llt(I);
        Eigen::VectorXd lambda = llt.solve(b);

        // 计算法曲率
        double h = 0;
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                h += II(i, j) * lambda[i] * lambda[j];

        double g = 0;
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                g += I(i, j) * lambda[i] * lambda[j];

        // 法曲率，法曲率圆心，投影点 Q
        double kappa = h / g;

        // 如果遇到曲率为零，就使用最近点投影（稳定性不太好）
        if (std::abs(kappa) < 1e-8)
        {
            ProjectWithNearest(surf, P, uv[0], uv[1], tol);
            break;
        }

        double R = std::abs(1 / kappa);
        gp_Vec O = gp_Vec(S00.XYZ()) + N / kappa;
        gp_Vec Q = O + (gp_Vec(P.XYZ()) - O).Normalized() * R;

        // 计算增量
        double dt = ((Q - gp_Vec(S00.XYZ())).Dot(S10 * lambda[0] + S01 * lambda[1])) > 0 ? 1 : -1;
        uv[0] += dt * lambda[0];
        uv[1] += dt * lambda[1];

        // 防止越界
        if (uv[0] <= U1)
            uv[0] = U1 + tol;
        if (uv[0] >= U2)
            uv[0] = U2 - tol;
        if (uv[1] <= V1)
            uv[1] = V1 + tol;
        if (uv[1] >= V2)
            uv[1] = V2 - tol;

        // 计算夹角
        angle = N.Crossed(dP).Magnitude() / dP.Magnitude();
        times++;
    }

    // 更新参数
    u = uv[0];
    v = uv[1];

    return angle;
}

Handle(Geom_BSplineSurface) OCCUtils::Chord(const std::vector<gp_Pnt> &cloud, std::vector<gp_Pnt2d> &uv, int Nu, int Nv,
                                            int p, int q, std::vector<double> &w, double alpha, double beta)
{
    // N 个控制点
    int nu = Nu - 1;
    int nv = Nv - 1;
    int m = cloud.size() - 1;

    // 节点向量
    std::vector<double> uKnots(Nu + p + 1);
    std::vector<double> vKnots(Nv + q + 1);
    double umax = std::max_element(uv.begin(), uv.end(), [](auto &a, auto &b) { return a.X() < b.X(); })->X() + 0.1;
    double umin = std::min_element(uv.begin(), uv.end(), [](auto &a, auto &b) { return a.X() < b.X(); })->X() - 0.1;
    double vmax = std::max_element(uv.begin(), uv.end(), [](auto &a, auto &b) { return a.Y() < b.Y(); })->Y() + 0.1;
    double vmin = std::min_element(uv.begin(), uv.end(), [](auto &a, auto &b) { return a.Y() < b.Y(); })->Y() - 0.1;

    // u 节点
    for (int i = 0; i < p + 1; i++)
        uKnots[i] = umin;
    for (int i = p + 1; i < Nu; i++)
        uKnots[i] = umin + (umax - umin) * (i - p) / (Nu - p);
    for (int i = Nu; i < Nu + p + 1; i++)
        uKnots[i] = umax;

    for (int i = 0; i < q + 1; i++)
        vKnots[i] = vmin;
    for (int i = q + 1; i < Nv; i++)
        vKnots[i] = vmin + (vmax - vmin) * (i - q) / (Nv - q);
    for (int i = Nv; i < Nv + q + 1; i++)
        vKnots[i] = vmax;

    // 填充最小二乘系数矩阵
    std::vector<Eigen::Triplet<double>> tripA;
    for (int k = 0; k < m + 1; k++)
    {
        double u = uv[k].X();
        double v = uv[k].Y();
        int uSpan = FindSpan(u, p, uKnots);
        int vSpan = FindSpan(v, q, vKnots);

        std::vector<double> Bu = BasisFuncs(uSpan, u, p, uKnots);
        std::vector<double> Bv = BasisFuncs(vSpan, v, q, vKnots);

        // 每次填充 (p + 1) * (q + 1) 规模的元素
        int uIndex = (uSpan - p) * (nv + 1);
        for (int i = 0; i < p + 1; i++)
        {
            int vIndex = vSpan - q;
            for (int j = 0; j < q + 1; j++)
                tripA.push_back(Eigen::Triplet<double>(k, uIndex + vIndex + j, Bu[i] * Bv[j]));

            uIndex += nv + 1;
        }
    }

    Eigen::SparseMatrix<double> A(m + 1, (nu + 1) * (nv + 1));
    A.setFromTriplets(tripA.begin(), tripA.end());

    // 构造系数矩阵，加上最小化弦长的权重，增加最小化切向变化的约束
    int Nuv = (nu + 1) * (nv + 1);
    double gamma = 1 - alpha - beta;

    // 先添加对角块
    std::vector<Eigen::Triplet<double>> tripB;
    for (int i = 0; i < nu + 1; i++)
    {
        int ii = i * (nv + 1);

        // 最小化弦长的边界
        tripB.push_back(Eigen::Triplet<double>(ii, ii, alpha));
        tripB.push_back(Eigen::Triplet<double>(ii, ii + 1, -alpha));

        tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv, alpha));
        tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv - 1, -alpha));

        // 最小化导数的边界
        tripB.push_back(Eigen::Triplet<double>(ii, ii, beta));
        tripB.push_back(Eigen::Triplet<double>(ii, ii + 1, -2 * beta));
        tripB.push_back(Eigen::Triplet<double>(ii, ii + 2, beta));

        tripB.push_back(Eigen::Triplet<double>(ii + 1, ii, -2 * beta));
        tripB.push_back(Eigen::Triplet<double>(ii + 1, ii + 1, 5 * beta));
        tripB.push_back(Eigen::Triplet<double>(ii + 1, ii + 2, -4 * beta));
        tripB.push_back(Eigen::Triplet<double>(ii + 1, ii + 3, beta));

        tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv, beta));
        tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv - 1, -2 * beta));
        tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv - 2, beta));

        tripB.push_back(Eigen::Triplet<double>(ii + nv - 1, ii + nv, -2 * beta));
        tripB.push_back(Eigen::Triplet<double>(ii + nv - 1, ii + nv - 1, 5 * beta));
        tripB.push_back(Eigen::Triplet<double>(ii + nv - 1, ii + nv - 2, -4 * beta));
        tripB.push_back(Eigen::Triplet<double>(ii + nv - 1, ii + nv - 3, beta));

        // 最小化弦长的中间部分
        for (int j = 1; j < nv; j++)
        {
            tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j, 2 * alpha));
            tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - 1, -alpha));
            tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + 1, -alpha));
        }

        // 最小化导数的中间部分
        for (int j = 2; j < nv - 1; j++)
        {
            tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j, 6 * beta));
            tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - 1, -4 * beta));
            tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + 1, -4 * beta));
            tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - 2, beta));
            tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + 2, beta));
        }

        // 添加最小化弦长的中间部分
        if (i > 0 && i < nu)
        {
            for (int j = 0; j < nv + 1; j++)
            {
                tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j, 2 * alpha));
                tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + nv + 1, -alpha));
                tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - (nv + 1), -alpha));
            }
        }

        // 添加最小化导数的中间部分
        if (i > 1 && i < nu - 1)
        {
            for (int j = 0; j < nv + 1; j++)
            {
                tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j, 6 * beta));
                tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + nv + 1, -4 * beta));
                tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - (nv + 1), -4 * beta));
                tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + (nv + 1) * 2, beta));
                tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - (nv + 1) * 2, beta));
            }
        }
    }

    // 处理前两行与最后两行
    for (int i = 0; i < nv + 1; i++)
    {
        // 第一行
        tripB.push_back(Eigen::Triplet<double>(i, i, alpha + beta));
        tripB.push_back(Eigen::Triplet<double>(i, i + nv + 1, -alpha - 2 * beta));
        tripB.push_back(Eigen::Triplet<double>(i, i + (nv + 1) * 2, beta));

        // 第二行
        tripB.push_back(Eigen::Triplet<double>(i + nv + 1, i, -2 * beta));
        tripB.push_back(Eigen::Triplet<double>(i + nv + 1, i + nv + 1, 5 * beta));
        tripB.push_back(Eigen::Triplet<double>(i + nv + 1, i + (nv + 1) * 2, -4 * beta));
        tripB.push_back(Eigen::Triplet<double>(i + nv + 1, i + (nv + 1) * 3, beta));

        // 最后一行
        int index = nu * (nv + 1) + i;
        tripB.push_back(Eigen::Triplet<double>(index, index, alpha + beta));
        tripB.push_back(Eigen::Triplet<double>(index, index - (nv + 1), -alpha - 2 * beta));
        tripB.push_back(Eigen::Triplet<double>(index, index - (nv + 1) * 2, beta));

        // 倒数第二行
        tripB.push_back(Eigen::Triplet<double>(index - nv - 1, index, -2 * beta));
        tripB.push_back(Eigen::Triplet<double>(index - nv - 1, index - nv - 1, 5 * beta));
        tripB.push_back(Eigen::Triplet<double>(index - nv - 1, index - (nv + 1) * 2, -4 * beta));
        tripB.push_back(Eigen::Triplet<double>(index - nv - 1, index - (nv + 1) * 3, beta));
    }

    Eigen::SparseMatrix<double> B((nu + 1) * (nv + 1), (nu + 1) * (nv + 1));
    B.setFromTriplets(tripB.begin(), tripB.end());

    Eigen::SparseMatrix<double> C = B + A.transpose() * A * gamma;

    // Cholesky 分解
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt(C);

    // 构造最小二乘+能量约束系统的右端矩阵
    Eigen::MatrixXd cv(m + 1, 3);
    for (int j = 0; j < m + 1; j++)
    {
        cv(j, 0) = cloud[j].X();
        cv(j, 1) = cloud[j].Y();
        cv(j, 2) = cloud[j].Z();
    }
    Eigen::MatrixXd b = A.transpose() * cv * gamma;

    // 求解线性系统，并获得控制点
    TColgp_Array2OfPnt cpts(1, nu + 1, 1, nv + 1);
    Eigen::MatrixXd X = ldlt.solve(b);
    for (int i = 0; i < nu + 1; i++)
    {
        for (int j = 0; j < nv + 1; j++)
        {
            int index = i * (nv + 1) + j;
            cpts.SetValue(i + 1, j + 1, {X(index, 0), X(index, 1), X(index, 2)});
        }
    }

    // 生成 OCC 的样条曲面
    TColStd_Array1OfReal UKnots(1, Nu - p + 1);
    TColStd_Array1OfReal VKnots(1, Nv - q + 1);
    TColStd_Array1OfInteger UMults(1, Nu - p + 1);
    TColStd_Array1OfInteger VMults(1, Nv - q + 1);

    for (int i = 0; i < Nu - p + 1; i++)
    {
        UKnots.SetValue(i + 1, uKnots[i + p]);
        UMults.SetValue(i + 1, 1);
    }

    for (int i = 0; i < Nv - q + 1; i++)
    {
        VKnots.SetValue(i + 1, vKnots[i + q]);
        VMults.SetValue(i + 1, 1);
    }

    // 两端重数
    UMults.SetValue(1, p + 1);
    UMults.SetValue(Nu - p + 1, p + 1);
    VMults.SetValue(1, q + 1);
    VMults.SetValue(Nv - q + 1, q + 1);

    return new Geom_BSplineSurface(cpts, UKnots, VKnots, UMults, VMults, p, q);
}

double OCCUtils::SurfaceEnergy(Handle(Geom_BSplineSurface) surface)
{
    int uN = surface->NbUPoles();
    int vN = surface->NbVPoles();
    int uDeg = surface->UDegree();
    int vDeg = surface->VDegree();

    auto &UKnots = surface->UKnots();
    auto &VKnots = surface->VKnots();
    auto &UMult = surface->UMultiplicities();
    auto &VMult = surface->VMultiplicities();

    std::vector<double> uknots;
    std::vector<double> vknots;
    for (int i = 1; i <= UKnots.Size(); i++)
    {
        int n = UMult.Value(i);
        for (int j = 0; j < n; j++)
        {
            uknots.push_back(UKnots.Value(i));
        }
    }

    for (int i = 1; i <= VKnots.Size(); i++)
    {
        int n = VMult.Value(i);
        for (int j = 0; j < n; j++)
        {
            vknots.push_back(VKnots.Value(i));
        }
    }

    // 计算区间长度
    double urange = std::abs(uknots.front() - uknots.back());
    double vrange = std::abs(vknots.front() - vknots.back());

    // 分别沿着 u,v 方向计算积分，然后保存
    Eigen::MatrixXd intBu0(uN, uN);
    Eigen::MatrixXd intBu1(uN, uN);
    Eigen::MatrixXd intBu2(uN, uN);
    Eigen::MatrixXd intBv0(vN, vN);
    Eigen::MatrixXd intBv1(vN, vN);
    Eigen::MatrixXd intBv2(vN, vN);

    // 初始化
    intBu0.setZero();
    intBu1.setZero();
    intBu2.setZero();
    intBv0.setZero();
    intBv1.setZero();
    intBv2.setZero();

    // 高斯积分
    std::vector<double> nodes = gaussNodes[2];
    std::vector<double> weights = gaussWeights[2];

    // 数值积分计算系数
    for (int i = uDeg; i < uN; i++)
    {
        double mid = (uknots[i + 1] + uknots[i]) / 2;
        double ratio = (uknots[i + 1] - uknots[i]) / 2;

        // 防御重节点，此时区间为零，不用积分
        if (std::abs(ratio) > 1e-8)
        {
            for (int j = 0; j < nodes.size(); j++)
            {
                auto Bu = OCCUtils::DerivBasisFuncs(2, i, mid + nodes[j] * ratio, uDeg, uknots);
                for (int ui = 0; ui < uDeg + 1; ui++)
                {
                    for (int uj = 0; uj < uDeg + 1; uj++)
                    {
                        intBu0(i - uDeg + ui, i - uDeg + uj) += Bu[0][ui] * Bu[0][uj] * weights[j] * ratio;
                        intBu1(i - uDeg + ui, i - uDeg + uj) += Bu[1][ui] * Bu[1][uj] * weights[j] * ratio;
                        intBu2(i - uDeg + ui, i - uDeg + uj) += Bu[2][ui] * Bu[2][uj] * weights[j] * ratio;
                    }
                }
            }
        }
    }

    // 数值积分计算系数
    for (int i = vDeg; i < vN; i++)
    {
        double mid = (vknots[i + 1] + vknots[i]) / 2;
        double ratio = (vknots[i + 1] - vknots[i]) / 2;

        // 防御重节点，此时区间为零，不用积分
        if (std::abs(ratio) > 1e-8)
        {
            for (int j = 0; j < nodes.size(); j++)
            {
                auto Bv = OCCUtils::DerivBasisFuncs(2, i, mid + nodes[j] * ratio, vDeg, vknots);
                for (int vi = 0; vi < vDeg + 1; vi++)
                {
                    for (int vj = 0; vj < vDeg + 1; vj++)
                    {
                        intBv0(i - vDeg + vi, i - vDeg + vj) += Bv[0][vi] * Bv[0][vj] * weights[j] * ratio;
                        intBv1(i - vDeg + vi, i - vDeg + vj) += Bv[1][vi] * Bv[1][vj] * weights[j] * ratio;
                        intBv2(i - vDeg + vi, i - vDeg + vj) += Bv[2][vi] * Bv[2][vj] * weights[j] * ratio;
                    }
                }
            }
        }
    }

    // 计算系数矩阵
    int N = uN * vN;
    Eigen::MatrixXd C(N, N);
    C.setZero();
    for (int i1 = 0; i1 < uN; i1++)
    {
        for (int j1 = 0; j1 < vN; j1++)
        {
            for (int i2 = 0; i2 < uN; i2++)
            {
                for (int j2 = 0; j2 < vN; j2++)
                {
                    C(i1 * vN + j1, i2 * vN + j2) += intBu2(i1, i2) * intBv0(j1, j2);
                    C(i1 * vN + j1, i2 * vN + j2) += 2 * intBu1(i1, i2) * intBv1(j1, j2);
                    C(i1 * vN + j1, i2 * vN + j2) += intBu0(i1, i2) * intBv2(j1, j2);
                }
            }
        }
    }

    // 计算控制点矩阵
    Eigen::MatrixXd X(N, 1);
    Eigen::MatrixXd Y(N, 1);
    Eigen::MatrixXd Z(N, 1);

    for (int i = 0; i < uN; i++)
    {
        for (int j = 0; j < vN; j++)
        {
            X(i * vN + j, 0) = surface->Pole(i + 1, j + 1).X();
            Y(i * vN + j, 0) = surface->Pole(i + 1, j + 1).Y();
            Z(i * vN + j, 0) = surface->Pole(i + 1, j + 1).Z();
        }
    }

    // 计算能量
    Eigen::MatrixXd R = X.transpose() * C * X + Y.transpose() * C * Y + Z.transpose() * C * Z;
    return R(0, 0);
}

void OCCUtils::MeasureDivation(Handle(Geom_BSplineSurface) surface,
                               const std::vector<Handle(Geom_Curve)> &boundaryCurves,
                               const std::vector<Handle(Geom_Surface)> &boundarySurfaces)
{
    int sample = 100;
    double maxG0 = 0, maxG1 = 0;
    for (int i = 0; i < boundaryCurves.size(); i++)
    {
        auto curve = boundaryCurves[i];
        double first = curve->FirstParameter();
        double second = curve->LastParameter();

        double G0 = 0;
        std::vector<std::tuple<double, double>> params;
        std::vector<gp_Pnt> points;
        for (int j = 0; j < sample; j++)
        {
            double t = first + (second - first) * j / (sample - 1);
            gp_Pnt P = curve->Value(t);

            double u, v;
            ProjectWithOrtho(surface, P, u, v, 1e-8);
            double d = ProjectWithNearest(surface, P, u, v, 1e-8);
            G0 += d;

            params.emplace_back(u, v);
            points.emplace_back(P);
        }
        G0 /= sample;
        maxG0 = std::max(maxG0, G0);

        auto adjSurface = boundarySurfaces[i];
        if (!adjSurface.IsNull())
        {
            double G1 = 0;
            for (int j = 0; j < sample; j++)
            {
                double u, v;
                gp_Pnt P = points[j];
                ProjectWithOrtho(adjSurface, P, u, v, 1e-8);
                ProjectWithNearest(adjSurface, P, u, v, 1e-8);

                gp_Vec DU, DV;
                adjSurface->D1(u, v, P, DU, DV);
                gp_Vec adjN = DU.Crossed(DV).Normalized();

                auto [u0, v0] = params[j];
                surface->D1(u0, v0, P, DU, DV);
                gp_Vec N = DU.Crossed(DV).Normalized();

                double angle = N.Angle(adjN) / M_PI * 180;
                G1 += std::min(angle, 180 - angle);
            }
            G1 /= sample;
            maxG1 = std::max(maxG1, G1);
        }
    }
    std::cout << "ave G0 error: " << maxG0 << std::endl;
    std::cout << "ave G1 error: " << maxG1 << std::endl;
}

void OCCUtils::CurveInfo(Handle(Geom_BSplineCurve) curve)
{
    auto deg = curve->Degree();
    auto knots = curve->KnotSequence();
    auto poles = curve->Poles();

    std::cout << "Degree: " << deg << std::endl;
    std::cout << "Knots: " << std::endl;
    for (int i = 1; i <= knots.Length(); i++)
        std::cout << knots.Value(i) << " ";
    std::cout << std::endl << "Poles: " << std::endl;
    for (int i = 1; i <= poles.Length(); i++)
        std::cout << poles.Value(i).X() << " " << poles.Value(i).Y() << " " << poles.Value(i).Z() << std::endl;
}

void OCCUtils::SurfaceInfo(Handle(Geom_BSplineSurface) surface)
{
    auto uDeg = surface->UDegree();
    auto vDeg = surface->VDegree();
    auto uKnots = surface->UKnotSequence();
    auto vKnots = surface->VKnotSequence();

    std::cout << "U Degree: " << uDeg << std::endl;
    std::cout << "V Degree: " << vDeg << std::endl;
    std::cout << "U Knots: " << std::endl;
    for (int i = 1; i <= uKnots.Length(); i++)
    {
        std::cout << uKnots.Value(i) << " ";
    }
    std::cout << std::endl << "V Knots: " << std::endl;
    for (int i = 1; i <= vKnots.Length(); i++)
    {
        std::cout << vKnots.Value(i) << " ";
    }

    auto poles = surface->Poles();
    std::cout << std::endl << "Poles: " << std::endl;
    for (int i = 1; i <= poles.NbRows(); i++)
    {
        for (int j = 1; j <= poles.NbColumns(); j++)
        {
            std::cout << poles.Value(i, j).X() << " " << poles.Value(i, j).Y() << " " << poles.Value(i, j).Z()
                      << std::endl;
        }
    }
}

bool OCCUtils::Tag(std::vector<std::vector<int>> &tags, int tag, int si, int sj, int depth)
{
    int size = tags.size();
    auto checkValid = [&](int tag, int si, int sj) {
        for (int i = -2; i < 3; i += 2)
            for (int j = -2; j < 3; j += 2)
                if (tags[si + i][sj + j] != 0 && tags[si + i][sj + j] != tag)
                    return false;
        return true;
    };
    if (depth > 0 && checkValid(tag, si, sj))
    {
        tags[si][sj] = tag;

        // 记录周围可标记的位置
        std::vector<std::vector<int>> pair;
        for (int i = -1; i < 2; i++)
        {
            for (int j = -1; j < 2; j++)
            {
                if (si + i < 2 || si + i > size - 3 || sj + j < 2 || sj + j > size - 3)
                    continue;

                if ((i != 0 || j != 0) && tags[si + i][sj + j] == 0)
                    pair.push_back({si + i, sj + j});
            }
        }

        // 随机选择扩张方向，随机增加深度
        while (!pair.empty())
        {
            int i = rand() % pair.size();
            Tag(tags, tag, pair[i][0], pair[i][1], depth - 1);
            pair.erase(pair.begin() + i);
        }
    }

    return tags[si][sj] == tag;
}

// 生成随机带洞网格
MeshPtr OCCUtils::GenerateRandomMesh(int size, int holes, std::function<Mesh::Point(double u, double v)> f)
{
    srand(time(0));
    MeshPtr mesh = std::make_unique<Mesh>();

    Mesh &meshRef = *mesh;
    meshRef.request_vertex_normals();
    meshRef.request_face_normals();
    meshRef.request_vertex_texcoords2D();

    // 去掉外围 2 层计算洞的深度
    int valid = size - 4;
    int depth = valid / holes / 2;

    // 移动有效区域，使洞的生成中心到边界的距离等于深度
    valid = size - depth * 2;

    // 标记空位置
    std::vector<std::vector<int>> tags(size, std::vector<int>(size, 0));
    for (int i = 0; i < holes;)
    {
        int tag = i + 1;
        int si = rand() % valid + depth;
        int sj = rand() % valid + depth;

        std::cout << "Pos: " << si << "," << sj << std::endl;
        if (Tag(tags, tag, si, sj, depth))
            i++;
    }

    double step = 1.0 / (size - 1);
    std::vector<Mesh::VertexHandle> vhandle1(size);
    std::vector<Mesh::VertexHandle> vhandle2(size);
    Mesh::VertexHandle h;
    for (int i = 0; i < size; i++)
    {
        for (int j = 0; j < size; j++)
        {
            if (tags[i][j] != 0)
                vhandle2[j] = h;
            else
            {
                vhandle2[j] = mesh->add_vertex(f(step * j, step * i));
                mesh->set_texcoord2D(vhandle2[j], {step * j, step * i});
            }

            if (i > 0 && j < size - 1)
            {
                std::vector<Mesh::VertexHandle> face_vhandles;

                face_vhandles.push_back(vhandle1[j]);
                face_vhandles.push_back(vhandle1[j + 1]);
                face_vhandles.push_back(vhandle2[j]);

                if (std::find(face_vhandles.begin(), face_vhandles.end(), h) == face_vhandles.end())
                    mesh->add_face(face_vhandles);
            }

            if (i > 0 && j > 0)
            {
                std::vector<Mesh::VertexHandle> face_vhandles;

                face_vhandles.push_back(vhandle1[j]);
                face_vhandles.push_back(vhandle2[j]);
                face_vhandles.push_back(vhandle2[j - 1]);

                if (std::find(face_vhandles.begin(), face_vhandles.end(), h) == face_vhandles.end())
                    mesh->add_face(face_vhandles);
            }
        }
        vhandle1 = vhandle2;
    }

    mesh->update_face_normals();
    mesh->update_vertex_normals();

    return mesh;
}

MeshPtr OCCUtils::GenerateTestMesh(int size, int holes, std::function<int(const std::vector<gp_Pnt> &)> generator)
{
    std::vector<gp_Pnt> points;

    // M 是边界点个数
    int M = generator(points);
    int N = points.size();

    // 从 8 * 8 网格开始，使用双三次样条
    int Nu = 25;
    int Nv = 25;
    int p = 3;
    int q = 3;

    // 目标权重
    double alpha = 1e-8;
    double beta = 1e-3;

    // 边界点和周围点加权
    double kappa = 1e-3;
    std::vector<double> w(N);
    for (int i = 0; i < N; i++)
    {
        if (i < M)
            w[i] = 1 - kappa;
        else
            w[i] = kappa;
    }

    ParamBuilder paraBuilder(points);
    Handle(Geom_BSplineSurface) surface = paraBuilder.Build(Nu, Nv, p, q, w, alpha, beta, 3);

    std::function<Mesh::Point(double, double)> f = [&](double u, double v) -> Mesh::Point {
        gp_Pnt P;
        surface->D0(u, v, P);

        double x = P.X();
        double y = P.Y();
        double z = P.Z();

        return Mesh::Point(x, y, z);
    };

    return GenerateRandomMesh(size, holes, f);
}

std::vector<std::tuple<TopoDS_Edge, TopoDS_Face>> OCCUtils::GetAdjFaces(const TopoDS_Face &face,
                                                                        const TopoDS_Shape &shape)
{
    std::vector<std::tuple<TopoDS_Edge, TopoDS_Face>> pairs;

    // 构建 Edge -> Faces 的映射
    TopTools_IndexedDataMapOfShapeListOfShape edgeFaceMap;
    TopExp::MapShapesAndAncestors(shape, TopAbs_EDGE, TopAbs_FACE, edgeFaceMap);

    // 遍历 Face 的所有边
    for (TopExp_Explorer edgeExplorer(face, TopAbs_EDGE); edgeExplorer.More(); edgeExplorer.Next())
    {
        const TopoDS_Edge &edge = TopoDS::Edge(edgeExplorer.Current());
        std::vector<TopoDS_Face> adjacentFaces{TopoDS_Face{}};

        // 查找共享该边的所有 Face
        const TopTools_ListOfShape &faces = edgeFaceMap.FindFromKey(edge);
        for (TopTools_ListIteratorOfListOfShape it(faces); it.More(); it.Next())
        {
            const TopoDS_Face &otherFace = TopoDS::Face(it.Value());
            if (!face.IsSame(otherFace))
                adjacentFaces.push_back(otherFace);
        }
        pairs.emplace_back(edge, adjacentFaces.back());
    }

    return pairs;
}

void OCCUtils::WriteCloud(const std::vector<gp_Pnt> &points, const char *path)
{
    // 创建复合体
    TopoDS_Compound compound;
    BRep_Builder builder;
    builder.MakeCompound(compound);

    // 将每个点转换为一个顶点并添加到复合体中
    for (int i = 0; i < points.size(); ++i)
    {
        TopoDS_Vertex vertex = BRepBuilderAPI_MakeVertex(points[i]);
        builder.Add(compound, vertex);
    }

    // 将复合体导出为 BREP 文件
    BRepTools::Write(compound, (std::string(TEST_DATA_PATH "/") + std::string(path)).c_str());
}

TopoDS_Shape OCCUtils::WirteAndRead(const TopoDS_Shape &shape, const char *path)
{
    STEPControl_Writer writer;
    writer.Transfer(shape, STEPControl_AsIs);
    writer.Write(Standard_CString(path));

    // 创建读取器
    STEPControl_Reader reader;
    reader.ReadFile(Standard_CString(path));

    // 检查读取信息
    reader.PrintCheckLoad(Standard_True, IFSelect_ItemsByEntity);

    // 获得可翻译的根实体数量，然后翻译所有实体，获得成功的数量
    Standard_Integer NbRoots = reader.NbRootsForTransfer();
    Standard_Integer NbTrans = reader.TransferRoots();

    // 检查翻译信息
    reader.PrintCheckTransfer(Standard_True, IFSelect_ItemsByEntity);

    // 添加获得的形状
    for (int i = 1; i <= reader.NbShapes(); i++)
    {
        auto shape = reader.Shape(i);
        return shape;
    }
    return TopoDS_Shape();
}